/*
 * shortestpath.cpp
 *
 *  Created on: Feb 27, 2011
 *      Author: tqlong
 */

#include "shortestpath.h"
#include "pqueue.h"
#include <armadillo>
#include <iostream>
#include <fstream>
#include <limits>

using namespace arma;
#define DOUBLE_INFINITY std::numeric_limits<double>::infinity()
/** Dijkstra shortest path */

double ShortestPathMatrix::operator()(int s, int t)
{
  assert(C_.n_rows == C_.n_cols);

  int n = C_.n_rows, u = s;
  s_ = s; t_ = t;
  dist_ = std::vector<double>(n);
  prev_ = std::vector<int>(n, s);
  std::vector<int> visited(n, false);

  for (int i = 0; i < n; i++) dist_[i] = C_.at(s, i);
  prev_[s] = -1;
  visited[s] = true;

  while (u != -1) {
    double min = DOUBLE_INFINITY;
    u = -1;
    for (int i = 0; i < n; i++)
      if (!visited[i] && dist_[i] < min) {
        u = i;
        min = dist_[i];
      }
    //std::cout << "u = " << u << "\n";
    if (u != -1) {
      visited[u] = true;
      for (int i = 0; i < n; i++) {
        double d = dist_[u] + C_.at(u, i);
        if (dist_[i] > d) {
          dist_[i] = d;
          prev_[i] = u;
        }
      }
    }
  }

  u = t;
  path_.clear();
  while (u != -1) {
    //std::cout << "u = " << u << "\n";
    path_.push_back(u);
    u = prev_[u];
  }
  std::reverse(path_.begin(), path_.end());
  cost_ = dist_[t];
  return cost_;
}

void ShortestPathMatrix::generate(int n, double fe)
{
  C_ = randu(n, n) * n;
  mat E = randu(n, n);
  for (int i = 0; i < n; i++)
    C_.at(i, i) = 0;
  for (int i = 0; i < n; i++)
    for (int j = 0; j < n; j++)
      if (i != j && E.at(i, j) > fe)
        C_.at(i, j) = pow(10, 100);
}

void ShortestPathAdjacent::load(const std::string& fname)
{
  std::ifstream f(fname.c_str());
  load(f);
  f.close();
}

void ShortestPathAdjacent::save(const std::string& fname)
{
  std::ofstream f(fname.c_str());
  save(f);
  f.close();
}

void ShortestPathAdjacent::load(std::istream& f)
{
  int n, u, v;
  double c;
  char line[100];
  f >> n;
  //cout << "n = " << n << "\n";
  adjc_vertex_ = std::vector<std::vector<int> >(n);
  adjc_cost_ = std::vector<std::vector<double> >(n);
  while (!f.eof()) {
    f >> u >> v >> c;
    //cout << "u = " << u << " v = " << v << " c = " << c << "\n";
    adjc_vertex_[u].push_back(v);
    adjc_cost_[u].push_back(c);
    f.getline(line, 100);
  }
}

void ShortestPathAdjacent::save(std::ostream& os)
{
  int n = adjc_vertex_.size();
  os << n << "\n";
  for (int u = 0; u < n; u++) {
    for (unsigned int i = 0; i < adjc_vertex_[u].size(); i++) {
      os << u << " " << adjc_vertex_[u][i] << " " << adjc_cost_[u][i] << "\n";
    }
  }
}

void ShortestPathAdjacent::generate(int n, double fe)
{
  adjc_vertex_ = std::vector<std::vector<int> >(n);
  adjc_cost_ = std::vector<std::vector<double> >(n);
  mat A = randu(n, n) * n;
  mat E = randu(n, n);
  for (int i = 0; i < n; i++)
    A.at(i, i) = 0;
  for (int i = 0; i < n; i++)
    for (int j = 0; j < n; j++)
      if (i != j && E.at(i, j) <= fe) {
        adjc_vertex_[i].push_back(j);
        adjc_cost_[i].push_back(A.at(i, j));
      }
}

double ShortestPathAdjacent::operator()(int s, int t)
{
  int n = adjc_vertex_.size(), u = s;
  s_ = s; t_ = t;
  pqueue<int, double> dist;
  shortest_dist_ = std::vector<double>(n, 0);
  prev_ = std::vector<int>(n, s);

  for (int i = 0; i < n; i++) if (i != s)
    dist.push(i, DOUBLE_INFINITY);
  for (unsigned int i = 0; i < adjc_vertex_[s].size(); i++) {
    int v = adjc_vertex_[s][i];
    double d = adjc_cost_[s][i];
    if (v != s) dist.replace(v, d);
  }
  prev_[s] = -1;

  while (dist.size() > 0) {
    double min = dist.min();
    int u = dist.minKey();
    dist.remove(u);
    shortest_dist_[u] = min;

    //std::cout << "u = " << u << "\n";
    for (unsigned int i = 0; i < adjc_vertex_[u].size(); i++) {
      int v = adjc_vertex_[u][i];
      //std::cout << "--> v = " << v << "\n";
      if (!dist.contains(v)) continue;
      double d = min + adjc_cost_[u][i];
      if (dist[v] > d) {
        dist.replace(v, d);
        prev_[v] = u;
      }
    }
  }

  u = t;
  path_.clear();
  while (u != -1) {
    //std::cout << "u = " << u << "\n";
    path_.push_back(u);
    u = prev_[u];
  }
  std::reverse(path_.begin(), path_.end());
  cost_ = shortest_dist_[t];
  return cost_;
}
